#include <new>
#include <memory>
#include <stdexcept>
#include <cmath>
#include <cstdlib>
#include <iostream>
#include <sstream>
#include <ctime>
#include <boost/shared_array.hpp>
#include <boost/random.hpp>
#include <boost/random/random_device.hpp>
#include <libplayerc++/playerc++.h>
#include "MapStateValidityChecker.h"
#include "SimpleExecutor.h"
#include "pause.h"
#include "ptRRT.h"
#include "FrictionModelImpl.h"
#include "VisualizerImpl.h"

using namespace PlayerCc;
using namespace ompl::base;
using namespace ompl::geometric;

typedef ompl::base::SE2StateSpace::StateType SE2StateType;

class Controller {

  boost::random::random_device rng;
  boost::random::uniform_int_distribution<> coin;

  // player parameters
  PlayerClient    robot;
  SimulationProxy sim;
  MapProxy        map;
  Position2dProxy gocon;
  Graphics2dProxy g;

  // ompl parameters
  StateSpacePtr space;
  SimpleSetup ss;

  boost::shared_ptr<FrictionModelImpl> frictionModel;

public:

  Controller() :
    coin(0,1),
    robot("localhost"),
    sim(&robot, 0),
    map(&robot, 0),
    gocon(&robot, 100),
    g(&robot, 0),
    space(new SE2StateSpace()),
    ss(space)
  {
    robot.Read();
    map.RequestMap();

    // set bounds
    double xmax = map.GetWidth() * map.GetResolution() / 2.0;
    double ymax = map.GetHeight() * map.GetResolution() / 2.0;
    RealVectorBounds bounds(2);
    bounds.setLow(0, -xmax); bounds.setHigh(0, xmax);
    bounds.setLow(1, -ymax); bounds.setHigh(1, ymax);
    space->as<SE2StateSpace>()->setBounds(bounds);

    // set validity checker
    StateValidityCheckerPtr checker(new MapStateValidityChecker(0.4, ss.getSpaceInformation(), map));
    ss.setStateValidityChecker(checker);

    //
    frictionModel.reset(new FrictionModelImpl(sim));
  }

private:

  PlannerPtr getPtRRTPlanner() {
    PlannerPtr planner(new ptRRT(ss.getSpaceInformation(), frictionModel));
    planner->as<ptRRT>()->setRange(0.5);
    return planner;
  }

  PlannerPtr getRRTPlanner() {
    PlannerPtr planner(new RRT(ss.getSpaceInformation()));
    planner->as<RRT>()->setRange(0.5);
    return planner;
  }

  bool evalPath(const PathGeometric & p) const {
    std::size_t stateCount = p.getStateCount();
    // state 1 is start, no water
    for (std::size_t i = 1; i < stateCount; ++i) {
      const State * state = p.getState(i);
      const SE2StateType * se2state = dynamic_cast<const SE2StateType*>(state);
      if (frictionModel->is_spill_truth(se2state->getX(), se2state->getY())) {
        return true;
      }
    }
    return false;
  }

  // plan motion from one start to one goal and eval
  // return true if path crosses water, false if not 
  bool planAndEval(PlannerPtr planner, const std::string & description,
    double startX, double startY,
    double goalX, double goalY) {
    // get current robot positions etc.
    robot.Read();

    // start at the existing position
    ScopedState<SE2StateSpace> start(ss.getSpaceInformation());
    start->setX(startX); start->setY(startY);

    // goal is the standby position
    ScopedState<SE2StateSpace> goal(ss.getSpaceInformation());
    goal->setX(goalX); goal->setY(goalY);

    // visualize
    std::cout << description.c_str() << " (" << goalX << ", " << goalY << ")" << std::endl;

    // set start and goal
    ss.setStartAndGoalStates(start, goal);

    // setup the path planner
    ss.setPlanner(planner);
    ss.clear();
    ss.setup();

    // plan
    PlannerStatus solved = ss.solve();

    // execute
    if (solved) {
      return evalPath(static_cast<PathGeometric&>(ss.getSolutionPath()));
    }

    throw std::runtime_error("cannot solve");
  }

  bool planAndEval(PlannerPtr planner, const std::string & description,
    const double p1[2], const double p2[2]) {
    return planAndEval(planner, description, p1[0], p1[1], p2[0], p2[1]);
  }

public:

  // control loop
  void run() {
    const double p1[2] = { 2.0, 0.0 };
    const double p2[2] = { -4.0, 0.0 };
    const int N = 50;
    // higher score is worse
    int rrtScore = 0;
    int ptrrtScore = 0;
    for (int i = 0; i < N; ++i) {
      if (planAndEval(getRRTPlanner(), "Point", p1, p2)) { ++rrtScore; } 
      if (planAndEval(getRRTPlanner(), "Point", p2, p1)) { ++rrtScore; }
      if (planAndEval(getPtRRTPlanner(), "Point", p1, p2)) { ++ptrrtScore; }
      if (planAndEval(getPtRRTPlanner(), "Point", p2, p1)) { ++ptrrtScore; }
    }
    // finish, calculate percentage of water hits
    double rrtRate = (double) rrtScore / (2.0 * N);
    double ptrrtRate = (double) ptrrtScore / (2.0 * N);
    // report
    std::cout << "hit water, rrt = " << rrtScore << " (" << rrtRate << "), pt-rrt = " << ptrrtScore << " (" << ptrrtRate << ")" << std::endl;
  }
};

int
main(int argc, char *argv[])
{
  try {
    Controller control;
    control.run();
    return 0;
  } catch (std::exception &e) {
    std::cerr << e.what() << std::endl;
    return 1;
  }
}
